import pandas as pd
import matplotlib.pyplot as plt
from sklearn.linear_model import LinearRegression
import numpy as np
import yaml

def parse_ksxt_line(line):
    """Parse a single KSXT line into its components."""
    line = line[2:len(line) - 1]
    parts = line.strip().split(',')
    try:
        lon = float(parts[2])
        lat = float(parts[3])
        return {'Longitude': lon, 'Latitude': lat}
    except (IndexError, ValueError):
        return None

def read_ksxt_file(file_path):
    """Read and parse all valid KSXT lines from the file, removing duplicates."""
    data = []
    seen_points = set()  # To keep track of unique points.
    with open(file_path, 'r') as file:
        for line in file:
            parsed_data = parse_ksxt_line(line)
            if parsed_data is not None:
                point_tuple = (parsed_data['Longitude'], parsed_data['Latitude'])
                if point_tuple not in seen_points:
                    seen_points.add(point_tuple)
                    data.append(parsed_data)
    return pd.DataFrame(data)

selected_points = []  # List to hold selected points

def onclick(event):
    """Event handler for mouse click events on the plot."""
    global ax, fig
    if event.inaxes is not None:
        # Get the coordinates of the clicked position
        clicked_lon, clicked_lat = event.xdata, event.ydata
        print(f"Clicked position: Longitude={clicked_lon}, Latitude={clicked_lat}")
        
        # Add red marker at the clicked position
        ax.scatter(clicked_lon, clicked_lat, s=50, c='red', marker='o')
        fig.canvas.draw_idle()
        
        # Save the clicked position to the list
        selected_points.append({'Longitude': clicked_lon, 'Latitude': clicked_lat})
        
        # Optionally save to file after each click or provide a separate function to do so
        save_selected_points_to_file('selected_points.txt')

def save_selected_points_to_file(filename):
    """Save the selected points to a file."""
    with open(filename, 'w') as f:
        for point in selected_points:
            f.write(f"{point['Longitude']},{point['Latitude']}\n")
    print(f"Selected points saved to {filename}")

def plot_points(df):
    """Plot points from DataFrame and enable clicking on the plot to get GPS positions."""
    global fig, ax
    fig, ax = plt.subplots(figsize=(10, 8))
    ax.scatter(df['Longitude'], df['Latitude'], s=10, c='blue', marker='o')
    plt.title('KSXT Data Points Without Duplicates')
    plt.xlabel('Longitude')
    plt.ylabel('Latitude')
    plt.grid(True)
    
    # Connect the click event to our handler
    cid = fig.canvas.mpl_connect('button_press_event', onclick)

    plt.show()

if __name__ == "__main__":
    ksxt_df = read_ksxt_file(r'C:\Users\17963\Desktop\ros_workspace\navigation2_tutorials\my_test_dir\gps\rtk_data_0.txt')
    print(ksxt_df)
    plot_points(ksxt_df)